#include "table.h"

Table::Table() {
	ll = vec2(0,0);
	ur = vec2(0,0);
	balls = vector<Ball>();
	BALL_RADIUS = 0.0;
	p = Physics();
}

Table::Table(vec2 lowerLeft, vec2 upperRight) {
	ll = lowerLeft;
	ur = upperRight;
	balls = vector<Ball>();
	BALL_RADIUS = 0.05;
	p = Physics();
}

/* Draw the table and the balls */
void Table::drawTable() {

	/*
	// draws a 2D table
	glPushMatrix();
	glTranslated(0,-0.1,0);
	glRotated(-90,1,0,0);
	glColor3f(0.0,0.0,0.0);
    glBegin(GL_LINE_LOOP);
    glVertex2d(ll[0],ll[1]);
    glVertex2d(ll[0],ur[1]);
    glVertex2d(ur[0],ur[1]);
    glVertex2d(ur[0],ll[1]);
    glEnd();
	glPopMatrix();
	*/

	// draw 3D balls
	glColor3d(1,1,1);
	for (int i = 0; i < balls.size(); i++) {
		balls[i].draw(BALL_RADIUS);
	}
	glColor3d(0,1,0);

}

/* Adds a ball to the table if it is valid */
void Table::addBall(Ball b) {
	if (legalBall(b))
		balls.push_back(b);
}

/* Returns false if the ball is not valid and cannot be placed */
bool Table::legalBall(Ball b) {

	vec2 minBox = b.getLocation() - vec2(BALL_RADIUS);
	vec2 maxBox = b.getLocation() + vec2(BALL_RADIUS);

	// if it is outside the table
	if (minBox[0] < ll[0] || minBox[1] < ll[1] || maxBox[0] > ur[0] || maxBox[1] > ur[1])
		return false;
	// if it overlaps with another ball
	for (vector<Ball>::iterator it = balls.begin(); it != balls.end(); ++it) {
		if (dist(it->getLocation(), b.getLocation()) < 2*BALL_RADIUS)
			return false;
		if (it->getID() == b.getID())
			return false;
	}

	return true;
}

/* Simulate ball movements -- MAIN FUNCTION */
void Table::simulate(double time) {
	while (time > 0) {
		double maxV = findMaxVelocity();
		double maxStep = tInterval();
		double ballCollision = findCollidingBallsInterval();
		double wallCollision = findCollidingWallInterval();
		double nextColl = min(ballCollision, wallCollision);
		if (time < nextColl && time < maxStep) {
			moveBalls(time);
			return;
		} else if (maxStep < nextColl) {
			moveBalls(maxStep);
			time -= maxStep;
		} else {
			moveBalls(nextColl);
			fixBallCollisions();
			fixWallCollisions();
			time -= nextColl;
		}
	}
}

/* Return the distance between two points */
double Table::dist(vec2 loc1, vec2 loc2) {
	return sqrt(pow(loc1[0] - loc2[0], 2) + pow(loc1[1] - loc2[1], 2));
}

/* Returns the maximum velocity of all the balls */
double Table::findMaxVelocity() {
	double maxV = 0;
	for (vector<Ball>::iterator it = balls.begin(); it != balls.end(); ++it) {
		if (it->velocity() > maxV)
			maxV = it->velocity();
	}
	return maxV;
}

/* Returns the time interval until the next ball collision */
double Table::findCollidingBallsInterval() {
	
	double minInterval = numeric_limits<double>::infinity();
	double temp;

	for (vector<Ball>::iterator curr = balls.begin(); curr != balls.end(); ++curr) {
		for (vector<Ball>::iterator it = balls.begin(); it != balls.end(); ++it) {
			if (curr->getID() != it->getID()) {
				if (dist(curr->getLocation(), it->getLocation()) <= 6 * BALL_RADIUS + 0.001) {
				
					// calculate collision time
					double a[] = {curr->getLocation()[0], curr->getLocation()[1],
						curr->getSpeed()[0], curr->getSpeed()[1]};
					double b[] = {it->getLocation()[0], it->getLocation()[1],
						it->getSpeed()[0], it->getSpeed()[1]};
					temp = p.collide(a,b,BALL_RADIUS);
					if (temp < minInterval)
						minInterval = temp;
				}
			}
		}
	}
	
	return minInterval;
}

/* Returns the time interval for the next wall collision */
double Table::findCollidingWallInterval() {

	// look at balls just beyond the radius since bounds are not inclusive
	double d = 3 * BALL_RADIUS + .05;
	double minInterval = numeric_limits<double>::infinity();
	double Vx, Vy, x, y, temp;
	double llx = ll[0];
	double lly = ll[1];
	double urx = ur[0];
	double ury = ur[1];
	vec2 minBox, maxBox;
	
	// find all balls that will hit the wall, then the min out of these
	for (vector<Ball>::iterator it = balls.begin(); it != balls.end(); ++it) {

		Vx = it->getSpeed()[0];
		Vy = it->getSpeed()[1];
		x = it->getLocation()[0];
		y = it->getLocation()[1];
		minBox = it->getLocation() - vec2(BALL_RADIUS);
		maxBox = it->getLocation() + vec2(BALL_RADIUS);
		
		// top
		if (minBox[0] >= llx && minBox[1] >= (ury - d) 
			&& maxBox[0] <= urx && maxBox[1] <= ury) {
				if (Vy <= 0)
					temp = numeric_limits<double>::infinity();
				else
					temp = (ury - BALL_RADIUS - y) / Vy;
				if (temp < minInterval)
					minInterval = temp;
		}
		// bottom
		if (minBox[0] >= llx && minBox[1] >= lly 
			&& maxBox[0] <= urx && maxBox[1] <= (lly + d)) {
				if (Vy >= 0)
					temp = numeric_limits<double>::infinity();
				else
					temp = (lly + BALL_RADIUS - y) / Vy;
				if (temp < minInterval)
					minInterval = temp;
		}
		// left
		if (minBox[0] >= llx && minBox[1] >= lly 
			&& maxBox[0] <= llx + d && maxBox[1] <= ury) {
				if (Vx >= 0)
					temp = numeric_limits<double>::infinity();
				else
					temp = (llx + BALL_RADIUS - x) / Vx;
				if (temp < minInterval)
					minInterval = temp;
		}
		// right
		if (minBox[0] >= (urx - d) && minBox[1] >= lly 
			&& maxBox[0] <= urx && maxBox[1] <= ury) {
				if (Vx <= 0)
					temp = numeric_limits<double>::infinity();
				else
					temp = (urx - BALL_RADIUS - x) / Vx;
				if (temp < minInterval)
					minInterval = temp;
		}
	}
	return minInterval;
}

/* Move all balls in the given time */
void Table::moveBalls(double time) {
	for (vector<Ball>::iterator it = balls.begin(); it != balls.end(); ++it) {
		double newX = it->getLocation()[0] + it->getSpeed()[0]*time;
		double newY = it->getLocation()[1] + it->getSpeed()[1]*time;
		it->changeLocation(newX, newY);
	}
}

/* Fix all the velocities of balls colliding with balls */
void Table::fixBallCollisions() {
	for (vector<Ball>::iterator curr = balls.begin(); curr != balls.end(); ++curr) {
		for (vector<Ball>::iterator it = balls.begin(); it != balls.end(); ++it) {
			if (curr->getID() != it->getID()) {
				if (dist(curr->getLocation(), it->getLocation()) <= 2 * BALL_RADIUS + 0.001) {
						double a[] = {curr->getLocation()[0], curr->getLocation()[1],
							curr->getSpeed()[0], curr->getSpeed()[1]};
						double b[] = {it->getLocation()[0], it->getLocation()[1],
							it->getSpeed()[0], it->getSpeed()[1]};
						p.rebound(a, b);
						curr->changeVelocity(a[2], a[3]);
						it->changeVelocity(b[2], b[3]);

				}
			}
		}
	}
}

/* Fix all the velocities of balls colliding with walls */
void Table::fixWallCollisions() {
	
	// get balls slightly beyond radius since bounds not always inclusive
	double d = 2*BALL_RADIUS + 0.0001;
	double Vx, Vy, x, y;
	double llx = ll[0];
	double lly = ll[1];
	double urx = ur[0];
	double ury = ur[1];
	vec2 minBox, maxBox;
	
	// find all balls that will hit the wall, then the min out of these
	for (vector<Ball>::iterator it = balls.begin(); it != balls.end(); ++it) {

		// needs to update after each collision in case of corners
		Vx = it->getSpeed()[0];
		Vy = it->getSpeed()[1];
		x = it->getLocation()[0];
		y = it->getLocation()[1];
		minBox = it->getLocation() - vec2(BALL_RADIUS);
		maxBox = it->getLocation() + vec2(BALL_RADIUS);
		
		// top
		if (minBox[0] >= llx && minBox[1] >= (ury - d) 
			&& maxBox[0] <= urx && maxBox[1] <= ury) {
				if (Vy > 0)
					it->changeVelocity(Vx, -1*Vy);
		}

		Vx = it->getSpeed()[0];
		Vy = it->getSpeed()[1];
		x = it->getLocation()[0];
		y = it->getLocation()[1];
		minBox = it->getLocation() - vec2(BALL_RADIUS);
		maxBox = it->getLocation() + vec2(BALL_RADIUS);

		// bottom
		if (minBox[0] >= llx && minBox[1] >= lly 
			&& maxBox[0] <= urx && maxBox[1] <= (lly + d)) {
				if (Vy < 0)
					it->changeVelocity(Vx, -1*Vy);
		}
		
		Vx = it->getSpeed()[0];
		Vy = it->getSpeed()[1];
		x = it->getLocation()[0];
		y = it->getLocation()[1];
		minBox = it->getLocation() - vec2(BALL_RADIUS);
		maxBox = it->getLocation() + vec2(BALL_RADIUS);

		// left
		if (minBox[0] >= llx && minBox[1] >= lly 
			&& maxBox[0] <= llx + d && maxBox[1] <= ury) {
				if (Vx < 0)
					it->changeVelocity(-1*Vx, Vy);
		}

		Vx = it->getSpeed()[0];
		Vy = it->getSpeed()[1];
		x = it->getLocation()[0];
		y = it->getLocation()[1];
		minBox = it->getLocation() - vec2(BALL_RADIUS);
		maxBox = it->getLocation() + vec2(BALL_RADIUS);

		// right
		if (minBox[0] >= (urx - d) && minBox[1] >= lly 
			&& maxBox[0] <= urx && maxBox[1] <= ury) {
				if (Vx > 0)
					it->changeVelocity(-1*Vx, Vy);
		}
	}
}

/* Return the maximum step interval */
double Table::tInterval() {
	return ((2 * BALL_RADIUS) / findMaxVelocity());
}